Program Listing for File feature_tracker.h

Return to documentation for file (codes/slam/featureTracker/feature_tracker.h)

/*******************************************************
 * Copyright (C) 2019, Robotics Group, Nanyang Technology University
 *
 * This file is part of sslam.
 *
 * Licensed under the GNU General Public License v3.0;
 * you may not use this file except in compliance with the License.
 *
 * Author: Zhang Handuo (hzhang032@e.ntu.edu.sg)
 *******************************************************/

#pragma once

#include <cstdio>
#include <iostream>
#include <queue>
#include <execinfo.h>
#include <csignal>
#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>
#include <opencv2/cudaoptflow.hpp>
#include <opencv2/cudaimgproc.hpp>
#include <opencv2/cudaarithm.hpp>

#include "camodocal/camera_models/CameraFactory.h"
#include "camodocal/camera_models/CataCamera.h"
#include "camodocal/camera_models/PinholeCamera.h"
#include "../estimator/parameters.h"
#include "../utility/tic_toc.h"

using namespace std;
using namespace camodocal;
using namespace Eigen;

namespace slam_estimator {

    inline double distance(cv::Point2f pt1, cv::Point2f pt2) {
        //printf("pt1: %f %f pt2: %f %f\n", pt1.x, pt1.y, pt2.x, pt2.y);
        double dx = pt1.x - pt2.x;
        double dy = pt1.y - pt2.y;
        return sqrt(dx * dx + dy * dy);
    }

    void reduceVector(vector<cv::Point2f> &v, vector<uchar> status);

    void reduceVector(vector<int> &v, vector<uchar> status);

    class FeatureTracker {
    public:
#ifndef DOXYGEN_SHOULD_SKIP_THIS

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#endif /* DOXYGEN_SHOULD_SKIP_THIS */

        FeatureTracker();

        map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> trackImage(double _cur_time, const cv::Mat &_img,
                                                                            const cv::Mat &_img1 = cv::Mat(),
                                                                            const cv::Mat &_mask = cv::Mat());

        void setMask();

        void readIntrinsicParameter(const vector<string> &calib_file);

        void showUndistortion(const string &name);


        void rejectWithF();

        static vector<cv::Point2f> undistortedPts(vector<cv::Point2f> &pts, camodocal::CameraPtr cam);

        vector<cv::Point2f> ptsVelocity(vector<int> &ids, vector<cv::Point2f> &pts,
                                        map<int, cv::Point2f> &cur_id_pts, map<int, cv::Point2f> &prev_id_pts);

        void showTwoImage(const cv::Mat &img1, const cv::Mat &img2,
                          vector<cv::Point2f> pts1, vector<cv::Point2f> pts2);

        void drawTrack(const cv::Mat &imLeft, const cv::Mat &imRight,
                       vector<int> &curLeftIds,
                       vector<cv::Point2f> &curLeftPts,
                       vector<cv::Point2f> &curRightPts,
                       map<int, cv::Point2f> &prevLeftPtsMap);

        void setPrediction(map<int, Eigen::Vector3d> &predictPts);

        void removeOutliers(set<int> &removePtsIds);

        cv::Mat getTrackImage();

        bool inBorder(const cv::Point2f &pt);

        //* row number (height) of the input image.
        int row;
        //* column number (width) of the input image.
        int col;

        //* Image for visualization (with circles and lines to display feature detection and optical flows)
        cv::Mat imTrack;
        //* Image of mask (to filter out dynamic objects and feature associations that are too far away).
        cv::Mat mask;
//    cv::Mat fisheye_mask;
        cv::Mat prev_img, cur_img, dy_mask, dilate_mask_inv;
        vector<cv::Point2f> n_pts;
        vector<cv::Point2f> predict_pts;
        vector<cv::Point2f> predict_pts_debug;
        vector<cv::Point2f> prev_pts, cur_pts, cur_right_pts;
        vector<cv::Point2f> prev_un_pts, cur_un_pts, cur_un_right_pts;
        vector<cv::Point2f> pts_velocity, right_pts_velocity;
        vector<int> ids, ids_right;
        vector<int> track_cnt;
        map<int, cv::Point2f> cur_un_pts_map, prev_un_pts_map;
        map<int, cv::Point2f> cur_un_right_pts_map, prev_un_right_pts_map;
        map<int, cv::Point2f> prevLeftPtsMap;
        vector<camodocal::CameraPtr> m_camera;
        double cur_time;
        double prev_time;
        bool stereo_cam;
        int n_id;
        bool hasPrediction;
    };
}